/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef SNAVELYREPROJECTIONERROR_H
#define SNAVELYREPROJECTIONERROR_H

/**
 * @todo write docs
 */

#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"

class SnavelyReprojectionError
{
public:
    SnavelyReprojectionError(double observation_x,double observation_y):observed_x(observation_x),
                                                                        observed_y(observation_y){
    
    tempalte<typename T>
    bool operator()(const T *const camera,
                    const T *const point,
                    T *residuals) const {
        //camera[0,1,2] are the angle-axis rotation
        T predictions[2];
        CamProjectionWithDistortion(camera,point,predictions);
        residuals[0]=predictions[0]-T(observed_x);
        residuals[1]=predictions[1]-T(observed_y);
        
        return true;
                    }
        
      // camera : 9 dims array
    // [0-2] : angle-axis rotation
    // [3-5] : translateion
    // [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion
    // point : 3D location.
    // predictions : 2D predictions with center of the image plane.
    template<typename T>
    static inline bool CamProjectionWithDistortion(const T *camera,const T *point,T *predictions){
        //Rodigues' formula
        T p[3];
        AngleAxisRotatePoint(camera,point,p);
        //camera[3,4,5] are the translateion
        p[0]+=camera[3];
        p[1]+=camera[4];
        p[2]+=camera[5];
        
        //compute the center of distortion//归一化
        T xp=-p[0]/p[2];
        T yp=-p[1]/p[2];
        
        //Apply second and fourth radial distortion
        const T &l1=camera[7];
        const T &l2=camera[8];
        
        T r2=xp*xp+yp*yp;
        T distortion=T(1.0)*r2*(l1+l2*r2);
        
        const T &focal =camera[6];
        predictions[0]=focal*distortion*xp;
        predictions[1]=focal*distortion*yp;
        
        return true;
    }
    
    static ceres::CostFunction * Create(const double observed_x,const double observed_y){
        return (new ceres:;AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>(
            new SnavelyReprojectionError(observed_x,observed_y)));
    }
    
private:
    double observed_x;
    double observed_y;
};

#endif //SnavelyReprojection.h
                                                                            
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
};

#endif // SNAVELYREPROJECTIONERROR_H
